void init_mqueryplan()         // initialize structures
{
#ifdef TOSSIM
  int i, k;
#endif  

  memset(querysource_buffer, 0, sizeof(querysource_buffer));
  memset(query_buffer, 0, sizeof(query_buffer));
  memset(queryjointarget_buffer, 0, sizeof(queryjointarget_buffer));
  if (query_getjoinnode(BASESTATION_ADDR, 0, NULL, 0) < 0)
  {                           // initialize JOININDEX_BASE = 0
    DBGERR("Cannot allocate join index for Base!");
    pause_sim();
  }  

#ifdef TOSSIM
  if (config.load_trees)
  {
    for (i = 0; i < config.queryspec_count; i++)
      handle_querymsghi(TOS_NODE_ID, TOS_NODE_ID, 0, NULL, config.query_spec[i].buffer_len,
        config.query_spec[i].buffer);
    
    if (config.start_route == 0)
    {
      DBGOUT("Loading join state...");
      load_joinstate();

      for (i = 0; i < QUERYBUFFER_SIZE; i++)
        if (query_buffer[i].used)
          for (k = 0; k < COSTBUFFER_SIZE; k++)
            if (query_buffer[i].groupopt.cost_buffer[k].used)
            {                   // reset sequences in cost buffer because innetplan_seq starts from 0
              query_buffer[i].groupopt.cost_buffer[k].cost.plancost_seq = 0;
              query_buffer[i].groupopt.cost_buffer[k].decision.decision_seq = 1;
            }

      optimize_plan();
    }
  }
#endif
}

void init_routepayload(uint8_t query_id, uint8_t predicate_count, ROUTING_PREDICATE *predicate,
  uint8_t eval_count, ATTRIBUTE *eval)
{                                          // initialize route payload to perform a long join
  INIT_MSGHI msg;
  uint8_t buffer[sizeof(msg)];
  int size;

DO_STACKTEST

  msg.cmd = CMDHI_INIT;
  msg.query_id = query_id;
  msg.path_len = 1;
  msg.path_levels[0] = trees[0].hops;
  msg.eval_count = eval_count;
  memcpy(msg.eval, eval, eval_count*sizeof(ATTRIBUTE));
    
  size = marshal_initmsghi(&msg, buffer);
  init_route(predicate_count, predicate, config.route_method, size, buffer);
}

void query_sourceinitiate(uint8_t query_id)  // query initiation, check which query needs to be performed, and start it up
{                                          // normal routing
  int i;
  RELSPEC_ENTRY *p;
  QUERYSPEC_ENTRY *p1;
  uint8_t tuple_len;
  ATTRIBUTE tuple[MAX_TUPLELEN];
  ROUTING_PREDICATE predicates[MAX_PREDICATES];
  STACK s;

DO_STACKTEST

  query_buffer[query_id].initiated = 1;

  p1 = &query_buffer[query_id].spec;
  if (p1->srcrel_count == 1)
  {
    DBGOUT("Single relation query, no need for initiation");
    return;
  }

  p = &p1->rel[REL_STATIC][REL_S];
  if (eval_stack(&p->pred, NULL, NULL, 0))
  {
    set_globalstatus(GS_ROUTE);  

    if (p->id_index < 0)
    {
      tuple_len = p->eval_count;
      for (i = 0; i < p->eval_count; i++)
        tuple[i] = eval_stack(&p->eval[i].eval, NULL, NULL, 0);
    } else {
      tuple_len = p->eval_count-1;
      for (i = 0; i < p->id_index; i++)
        tuple[i] = eval_stack(&p->eval[i].eval, NULL, NULL, 0);
      for (i = p->id_index+1; i < p->eval_count; i++)
        tuple[i-1] = eval_stack(&p->eval[i].eval, NULL, NULL, 0);
    }

    for (i = 0; i < p1->routingpredicate_count; i++)
    {
      memcpy(&s, &p1->routing_predicates[i].pred, sizeof(STACK));
      predicates[i].summary_index = p1->routing_predicates[i].summary_index;
      if (summary_desc[predicates[i].summary_index].type == SUMMARY_RTREE)
      {
        s.stack_len -= 3;
        predicates[i].value = eval_stack(&s, NULL, NULL, 0);
        s.stack_len = p1->routing_predicates[i].lastchild_ofs;
        predicates[i].value_ext = eval_stack(&s, NULL, NULL, 0);
      } else {
        s.stack_len -= 2;
        predicates[i].value = eval_stack(&s, NULL, NULL, 0);
        predicates[i].value_ext = 0;
      }
    }

    init_routepayload(query_id, p1->routingpredicate_count, predicates, tuple_len, tuple);
  }
}

uint8_t query_targetcheckactivate(uint8_t query_id, ADDRESS source_node, uint8_t tuplelen_s, ATTRIBUTE *tuple_s)
{                                                           // are we a target: 1 - yes
  int i;
  ATTRIBUTE tuple_t[MAX_TUPLELEN];
  QUERYSPEC_ENTRY *p;

DO_STACKTEST

  p = &query_buffer[query_id].spec;
  if (p->srcrel_count == 1)    // not a target for only one relation
  {
    DBGERR("Single relation query, should not be establishing paths");
    pause_sim();
    return 0;
  }

  if (eval_stack(&p->rel[REL_STATIC][REL_T].pred, NULL, NULL, 0))
  {
    if (p->rel[REL_STATIC][REL_S].id_index >= 0)
    {
      for (i = p->rel[REL_STATIC][REL_S].eval_count-1; i > p->rel[REL_STATIC][REL_S].id_index; i--)
        tuple_s[i] = tuple_s[i-1];
      tuple_s[p->rel[REL_STATIC][REL_S].id_index] = source_node;
      tuplelen_s++;
    }

    for (i = 0; i < p->rel[REL_STATIC][REL_T].eval_count; i++)
      tuple_t[i] = eval_stack(&p->rel[REL_STATIC][REL_T].eval[i].eval, NULL, NULL, 0);

    return (eval_stack(&p->rel[REL_STATIC][REL_J].pred, tuple_s, tuple_t, 0) != 0);
  }

  return 0;
}

uint8_t is_singlerel(uint8_t query_id, uint8_t rel_id)
{
  int i;

  for (i = 0; i < QUERYSOURCEBUFFER_SIZE; i++)
    if (querysource_buffer[i].used && querysource_buffer[i].query_id == query_id &&
      querysource_buffer[i].rel_id != rel_id)
      return 0;

  return 1;
}

uint8_t source_count(uint8_t query_id)
{
  int i;
  uint8_t result = 0;

  for (i = 0; i < QUERYSOURCEBUFFER_SIZE; i++)
    if (querysource_buffer[i].used && querysource_buffer[i].query_id == query_id)
      result++;

  return result;
}

int query_getjoinnode(ADDRESS join_node, uint8_t path_nodecount, ADDRESS *path_nodes, uint8_t level)
{                         // supplies a new join node, and path to it, and returns an index to it in the join target buffer
  int i;

DO_STACKTEST

  for (i = 0; i < QUERYJOINTARGETBUFFER_SIZE; i++)      // check if this join node already exists
    if (queryjointarget_buffer[i].used && queryjointarget_buffer[i].id == join_node)
    {                                           // only update path if it is an improvement
      if (path_nodecount < queryjointarget_buffer[i].path_nodecount)
      {
        if (queryjointarget_buffer[i].path_nodes)
        {
          mfree(queryjointarget_buffer[i].path_nodes);
          queryjointarget_buffer[i].path_nodes = NULL;
        }

        queryjointarget_buffer[i].path_nodecount = path_nodecount;
        if (path_nodecount > 0)
        {
          queryjointarget_buffer[i].path_nodes = (ADDRESS *) mcalloc(path_nodecount, sizeof(ADDRESS));
          memcpy(queryjointarget_buffer[i].path_nodes, path_nodes, path_nodecount*sizeof(ADDRESS));
        }

#ifdef TOSSIM
        DBGOUT("Updated path to join node %d", join_node);
        if (path_nodecount > 0)
          print_path(path_nodes, path_nodecount, "New path");
#endif
      }

      return i;
    }

  for (i = 0; i < QUERYJOINTARGETBUFFER_SIZE; i++)        // join node does not exists, add in buffer
    if (queryjointarget_buffer[i].used == 0)
    {
      queryjointarget_buffer[i].used = 1;
      queryjointarget_buffer[i].id = join_node;
      queryjointarget_buffer[i].level = level;
      memset(queryjointarget_buffer[i].tuples_sent, 0, sizeof(queryjointarget_buffer[i].tuples_sent));

      queryjointarget_buffer[i].path_nodecount = path_nodecount;
      if (path_nodecount > 0)
      {
        queryjointarget_buffer[i].path_nodes = (ADDRESS *) mcalloc(path_nodecount, sizeof(ADDRESS));
        memcpy(queryjointarget_buffer[i].path_nodes, path_nodes, path_nodecount*sizeof(ADDRESS));
      } else
        queryjointarget_buffer[i].path_nodes = NULL;

#ifdef TOSSIM
      DBGOUT("Set path to join node %d", join_node);
      if (path_nodecount > 0)
        print_path(path_nodes, path_nodecount, "Path");
#endif

      return i;
    }

  return -1;
}

int find_sourcenode(ADDRESS node_id)
{
  int i;

  for (i = 0; i < QUERYSOURCEBUFFER_SIZE; i++)
    if (querysource_buffer[i].used && querysource_buffer[i].id == node_id)
      return i;

  return -1;
}

int query_getsourcenode(uint8_t query_id, ADDRESS source_node, uint8_t rel_id, uint8_t path_len,
  ADDRESS *path, uint8_t *path_levels, uint8_t allow_add)
{                       // updates join pairs buffer, with a source node
  int i;

DO_STACKTEST

  for (i = 0; i < QUERYSOURCEBUFFER_SIZE; i++)
    if (querysource_buffer[i].used && querysource_buffer[i].query_id == query_id &&
      querysource_buffer[i].id == source_node && querysource_buffer[i].rel_id == rel_id)
      return i;

  if (allow_add)
    for (i = 0; i < QUERYSOURCEBUFFER_SIZE; i++)
      if (querysource_buffer[i].used == 0)
      {
        querysource_buffer[i].used = 1;
        querysource_buffer[i].query_id = query_id;
        querysource_buffer[i].id = source_node;
        querysource_buffer[i].seq_number = 0;
        querysource_buffer[i].rel_id = rel_id;
        querysource_buffer[i].pair_cost = 0xffffffff;
        querysource_buffer[i].jointarget_index = -1;
        memcpy(querysource_buffer[i].sel, query_buffer[query_id].spec.sel_est,
          sizeof(query_buffer[query_id].spec.sel_est));

        querysource_buffer[i].path_len = path_len;
        if (path_len > 0)
        {
          querysource_buffer[i].path = (ADDRESS *) mcalloc(path_len, sizeof(ADDRESS));
          memcpy(querysource_buffer[i].path, path, path_len*sizeof(ADDRESS));
          if (path_levels)
          {
            querysource_buffer[i].path_levels = (uint8_t *) mmalloc(path_len);
            memcpy(querysource_buffer[i].path_levels, path_levels, path_len);
          }
        }

        return i;
      }

  return -1;
}

uint8_t check_joinnodeimproved(uint8_t query_id, int querysource_index, uint8_t full_pathlen,
  ADDRESS *full_path, uint8_t *full_pathlevels, ADDRESS *best_joinnode, int *best_joinindex,
  uint32_t *best_cost, uint8_t threshold)
{
  int i;
  uint32_t cost;
  uint8_t updated = 0;
#ifdef TOSSIM
  char s[256] = "";
  int len;
#endif

DO_STACKTEST

  for (i = 0; i < full_pathlen; i++)        // evaluate all intermediate join nodes on the path, retain the best one
  {
    cost = estimate_paircost(&full_pathlevels[i], full_pathlen-i, full_pathlevels, i+1,
      full_path[i] == BASESTATION_ADDR, 1-querysource_buffer[querysource_index].rel_id,
      querysource_buffer[querysource_index].sel, query_buffer[query_id].tuple_len,
      query_buffer[query_id].spec.params[PARAM_WINDOWSIZE]);
    if ((threshold == 0 && cost < (*best_cost)) || (threshold && cost < 9*(*best_cost)/10))
    {
      *best_cost = cost;
      *best_joinindex = i;
      *best_joinnode = full_path[i];
      updated = 1;
    }
#ifdef TOSSIM
    len = strlen(s);
    sprintf(&s[len], "%ld ", cost);
#endif
  }

#ifdef TOSSIM
  DBGOUT("Full path cost: %s", s);
  print_sel(querysource_buffer[querysource_index].sel);
#endif

  return updated;
}

void select_joinnode(uint8_t query_id, int querysource_index, uint8_t full_pathlen,
  ADDRESS *full_path, uint8_t *full_pathlevels, ADDRESS best_joinnode, int best_joinindex,
  uint32_t best_cost, uint8_t sel_update)
{
  CONFIRMJOIN_MSGHI msg_out;
  CONFIRMTARGETSOURCE_MSGHI msg_out1; // in case of join at base station, notify other source directly
  uint8_t buffer[MAX(sizeof(msg_out), sizeof(msg_out1))];
  int size, queryjointarget_index, oldqueryjointarget_index;
  ADDRESS source_node = querysource_buffer[querysource_index].id;

DO_STACKTEST

  DBGOUT("Selected %d as join point", best_joinnode);
#ifdef TOSSIM
  log_querycostmodel(source_node, TOS_NODE_ID, best_joinnode, best_cost);
#endif

  querysource_buffer[querysource_index].pair_cost = best_cost;
  querysource_buffer[querysource_index].seq_number++;  // will wrap back to 0
  if (best_joinnode == BASESTATION_ADDR)   
  {                                  
    queryjointarget_index = JOININDEX_BASE;
    msg_out.path_nodecount = 0;

    if (source_node != BASESTATION_ADDR)   // if we join at Base, notify source directly, not through Base
    {
      msg_out1.cmd = CMDHI_CONFIRMTARGETSOURCE;
      msg_out1.query_id = query_id;
      msg_out1.seq_number = querysource_buffer[querysource_index].seq_number;
      msg_out1.sel_update = sel_update;
      if (sel_update)
        memcpy(msg_out1.sel_est, query_buffer[query_id].sel_est, sizeof(msg_out1.sel_est));

      DBGOUT("Notifying source %d to join at base station for query %d", source_node, query_id);
      size = marshal_confirmtargetsourcemsghi(&msg_out1, buffer);
      init_forward(TOS_NODE_ID, TOS_NODE_ID, source_node, full_pathlen-1, &full_path[1],
        FORWARD_NONDESTRUCTIVE, size, buffer, 0, NULL, ROUTE_SUCCESS);   // hack: pretend the base st. sent this message
    }
  } else {                                     // if we are joinining at some other node, tell it to notify the source
    queryjointarget_index = query_getjoinnode(best_joinnode, best_joinindex, &full_path[1],
      full_pathlevels[best_joinindex]);
    if (queryjointarget_index < 0)
    {
      DBGERR("Cannot find free slot in queryjointarget_buffer");
      pause_sim();
      return;
    }

    msg_out.path_nodecount = full_pathlen-best_joinindex-1;
    memcpy(msg_out.path_nodes, &full_path[best_joinindex+1], msg_out.path_nodecount*sizeof(ADDRESS));
  }

  oldqueryjointarget_index = querysource_buffer[querysource_index].jointarget_index;
  if (oldqueryjointarget_index >= 0 && oldqueryjointarget_index != queryjointarget_index)
  {
    msg_out.cmd = CMDHI_CONFIRMJOIN;    // notify old join node to remove pair
    msg_out.query_id = query_id;
    msg_out.remove_pair = 1;
    msg_out.source_node = source_node;

    DBGOUT("Notifying node %d to stop join between %d and %d on query %d",
      queryjointarget_buffer[oldqueryjointarget_index].id, source_node, TOS_NODE_ID, query_id);
    size = marshal_confirmjoinmsghi(&msg_out, buffer);
    init_forward(TOS_NODE_ID, TOS_NODE_ID, queryjointarget_buffer[oldqueryjointarget_index].id,
      queryjointarget_buffer[oldqueryjointarget_index].path_nodecount,
      queryjointarget_buffer[oldqueryjointarget_index].path_nodes, FORWARD_DESTRUCTIVE,
      size, buffer, 0, NULL, ROUTE_SUCCESS);
  }
  
  querysource_buffer[querysource_index].jointarget_index = queryjointarget_index;

#ifdef TOSSIM
  join_lognode(query_id, queryjointarget_buffer[queryjointarget_index].id, source_node);
  if (full_pathlen > 1)
    print_path(msg_out.path_nodes, msg_out.path_nodecount, "Path from join node to other source");
#endif

  msg_out.cmd = CMDHI_CONFIRMJOIN;    // in any case, notify the join node for our join pair
  msg_out.query_id = query_id;
  msg_out.remove_pair = 0;
  msg_out.source_node = source_node;
  msg_out.seq_number = querysource_buffer[querysource_index].seq_number;
  msg_out.sel_update = sel_update;
  if (sel_update)
    memcpy(msg_out.sel_est, query_buffer[query_id].sel_est, sizeof(msg_out.sel_est));

  DBGOUT("Notifying node %d to perform join between %d and %d on query %d",
    queryjointarget_buffer[queryjointarget_index].id, source_node, TOS_NODE_ID, query_id);
  size = marshal_confirmjoinmsghi(&msg_out, buffer);
  if (source_node == BASESTATION_ADDR && best_joinnode == BASESTATION_ADDR)  // we're joining with Base @ Base
    init_forward(TOS_NODE_ID, TOS_NODE_ID, BASESTATION_ADDR, full_pathlen-1, &full_path[1],
      FORWARD_NONDESTRUCTIVE, size, buffer, 0, NULL, ROUTE_SUCCESS);  //Base needs to know how to get to this node (for groupopt)
  else init_forward(TOS_NODE_ID, TOS_NODE_ID, queryjointarget_buffer[queryjointarget_index].id,
    queryjointarget_buffer[queryjointarget_index].path_nodecount,
    queryjointarget_buffer[queryjointarget_index].path_nodes,
    (best_joinnode == BASESTATION_ADDR) ? FORWARD_DESTRUCTIVE : FORWARD_NONDESTRUCTIVE,
    size, buffer, 0, NULL, ROUTE_SUCCESS);         // Base does not need to know how to get to this node
}

void handle_initmsghi(ADDRESS source_node, ADDRESS dest_node, uint8_t path_nodecount,
  ADDRESS *path_nodes, uint8_t data_len, uint8_t *data)  // handler for long join establishment
{
  INIT_MSGHI msg_in;
  int querysource_index, i, best_joinindex = 0;
  uint32_t best_cost;
  ADDRESS best_joinnode = 0;
  ADDRESS full_path[MAX_PATHLEN];
  uint8_t full_pathlevels[MAX_PATHLEN], full_pathlen;
  uint8_t new_pair = 0;

DO_STACKTEST

  unmarshal_initmsghi(&msg_in, data);
  if (query_buffer[msg_in.query_id].used == 0)
  {
    DBGERR("Query %d not used", msg_in.query_id);
    return;
  }
  if (query_targetcheckactivate(msg_in.query_id, source_node, msg_in.eval_count, msg_in.eval) == 0)
  {
    DBGOUT("Target activation condition not met");
    return;
  }

  full_pathlen = path_nodecount+1;
  memcpy(&full_path[1], path_nodes, path_nodecount*sizeof(ADDRESS));
  full_path[0] = TOS_NODE_ID;
  if (msg_in.path_len != full_pathlen)
  {
    DBGERR("Invalid path len: %d, %d", msg_in.path_len, full_pathlen);
    pause_sim();
    return;
  }

  for (i = 0; i < full_pathlen; i++)
    full_pathlevels[i] = msg_in.path_levels[full_pathlen-1-i];

#ifdef TOSSIM
  print_path(full_path, full_pathlen, "Full path");
  print_list(full_pathlevels, full_pathlen, "Full path levels");
#endif

  querysource_index = query_getsourcenode(msg_in.query_id, source_node, REL_T, path_nodecount,
    path_nodes, &full_pathlevels[1], 1);                     // update source buffer with the source node
  if (querysource_index < 0)
  {
    DBGERR("Cannot allocate querysource_buffer entry");
    pause_sim();
    return;
  }

  best_cost = querysource_buffer[querysource_index].pair_cost;
  if (best_cost == 0xffffffff)             // if this is the first time we encounter this source
  {                                        // evaluate join at base
    best_cost = estimate_paircost(&msg_in.path_levels[0], 1, &trees[0].hops, 1, 1, REL_S,
      query_buffer[msg_in.query_id].sel_est, query_buffer[msg_in.query_id].tuple_len,
      query_buffer[msg_in.query_id].spec.params[PARAM_WINDOWSIZE]);
    best_joinnode = BASESTATION_ADDR;
    new_pair = 1;
    
    DBGOUT("Base station cost: %d", best_cost);
    print_sel(query_buffer[msg_in.query_id].sel_est);
  }

  if (check_joinnodeimproved(msg_in.query_id, querysource_index, full_pathlen, full_path,
    full_pathlevels, &best_joinnode, &best_joinindex, &best_cost, 0) || new_pair)
  {                                       // warning: check_joinnodeimproved MUST be called, no shorcut evaluation
    select_joinnode(msg_in.query_id, querysource_index, full_pathlen, full_path, full_pathlevels,
      best_joinnode, best_joinindex, best_cost, 0);
    group_opt(msg_in.query_id, querysource_index);
  } else
    DBGOUT("No improvement to join cost");
}

void update_pairselest(uint8_t query_id, uint8_t querysource_index, uint8_t *sel)
{
  int i, j;
  uint32_t sum[REL_COUNT], count = 0;

DO_STACKTEST
  
  memcpy(querysource_buffer[querysource_index].sel, sel, sizeof(querysource_buffer[querysource_index].sel));
  memset(sum, 0, sizeof(sum));
  
  for (i = 0; i < QUERYSOURCEBUFFER_SIZE; i++)
    if (querysource_buffer[i].used && querysource_buffer[i].query_id == query_id)
    {
      count++;
      for (j = 0; j < REL_COUNT; j++)
        sum[j] += 1000000/((j == querysource_buffer[i].rel_id) ? query_buffer[query_id].sel_est[j] : querysource_buffer[i].sel[j]);
    }

  if (count > 0)
    for (j = 0; j < REL_COUNT; j++)
      query_buffer[query_id].sel_est[j] = (1000000*count)/sum[j];

  DBGOUT("Received selectivity for pair with %d: (%d, %d, %d)",
    querysource_buffer[querysource_index].id, sel[REL_S], sel[REL_T], sel[REL_J]);
  DBGOUT("Src selectivities updated to %d, %d, join selectivity updated to %d",
    query_buffer[query_id].sel_est[REL_S], query_buffer[query_id].sel_est[REL_T], query_buffer[query_id].sel_est[REL_J]);
}

uint8_t doupdate_joinnode(QUERY_ENTRY *q, int querysource_index)
{
  int best_joinindex;
  ADDRESS oldjoin_node, best_joinnode;
  uint32_t best_cost;
  uint8_t full_pathlen;
  ADDRESS full_path[MAX_PATHLEN];
  uint8_t full_pathlevels[MAX_PATHLEN];
  
  oldjoin_node = queryjointarget_buffer[querysource_buffer[querysource_index].jointarget_index].id;
  full_pathlen = querysource_buffer[querysource_index].path_len;
  memcpy(&full_path[1], querysource_buffer[querysource_index].path, full_pathlen*sizeof(ADDRESS));
  full_path[0] = TOS_NODE_ID;
  memcpy(&full_pathlevels[1], querysource_buffer[querysource_index].path_levels, full_pathlen);
  full_pathlevels[0] = trees[0].hops;
  full_pathlen++;

#ifdef TOSSIM
  DBGOUT("Old join node: %d", oldjoin_node);
  print_path(full_path, full_pathlen, "Full path");
  print_list(full_pathlevels, full_pathlen, "Full path levels");
#endif

  best_cost = estimate_paircost(&full_pathlevels[full_pathlen-1], 1, &full_pathlevels[0], 1, 1, REL_S,
    q->sel_est, q->tuple_len, q->spec.params[PARAM_WINDOWSIZE]);
  best_joinnode = BASESTATION_ADDR;

  DBGOUT("Base station cost: %d", best_cost);
  print_sel(q->sel_est);

  if (check_joinnodeimproved(q->query_id, querysource_index, full_pathlen, full_path,
    full_pathlevels, &best_joinnode, &best_joinindex, &best_cost, 1) && best_joinnode != oldjoin_node)
  {                                       // warning: check_joinnodeimproved MUST be called, no shorcut evaluation
    select_joinnode(q->query_id, querysource_index, full_pathlen, full_path, full_pathlevels,
      best_joinnode, best_joinindex, best_cost, 1);
    q->rel[querysource_buffer[querysource_index].rel_id].multicast.improved = 1;
    return 1;
  } else {
    DBGOUT("Join node unchanged");
    return 0;
  }
}

void handle_estupdatemsghi(ADDRESS source_node, ADDRESS dest_node, uint8_t path_nodecount,
  ADDRESS *path_nodes, uint8_t data_len, uint8_t *data)
{
  ESTUPDATE_MSGHI msg_in;
  int querysource_index;
  uint8_t groupopt_decision;
  QUERY_ENTRY *q;

DO_STACKTEST

  unmarshal_estupdatemsghi(&msg_in, data);
  q = &query_buffer[msg_in.query_id];
  if (q->used == 0)
  {
    DBGERR("Query %d not used", msg_in.query_id);
    return;
  }
  
  querysource_index = query_getsourcenode(q->query_id, msg_in.source_node, REL_T, 0, NULL, NULL, 0);
  if (querysource_index < 0)
  {
    DBGERR("Cannot find source node %d in querysource_buffer", source_node);
    pause_sim();
    return;
  }

  groupopt_decision = q->groupopt.decision;
  if ((groupopt_decision == DECISION_BASE && source_node != BASESTATION_ADDR) ||
    (groupopt_decision == DECISION_INNET &&
    queryjointarget_buffer[querysource_buffer[querysource_index].jointarget_index].id != source_node))
  {
    DBGOUT("Ignored redundant estimate from non-join node");
    return;
  }

  DBGOUT("Estimate update received from %d, source %d, query %d", source_node, msg_in.source_node,
    q->query_id);
  update_pairselest(q->query_id, querysource_index, msg_in.sel_est);

  if (doupdate_joinnode(q, querysource_index))
    group_opt(q->query_id, querysource_index);
}

void handle_confirmjoinmsghi(ADDRESS source_node, ADDRESS dest_node, uint8_t path_nodecount,
  ADDRESS *path_nodes, uint8_t data_len, uint8_t *data)
{                                     // message handler, the join point receives a msg from a target about a pair
  CONFIRMJOIN_MSGHI msg_in;
  CONFIRMSOURCE_MSGHI msg_out;
  uint8_t buffer[sizeof(msg_out)];
  int size;

DO_STACKTEST

  unmarshal_confirmjoinmsghi(&msg_in, data);
  if (query_buffer[msg_in.query_id].used == 0)
  {
    DBGERR("Query %d not used", msg_in.query_id);
    return;
  }
  
  if (msg_in.remove_pair)
  {
    DBGOUT("Removing pair between sources %d and %d on query %d", source_node, msg_in.source_node,
      msg_in.query_id);
    remove_joinpair(msg_in.query_id, msg_in.source_node, source_node, PAIROPT_INNET, 1);
  } else {
    DBGOUT("Will join sources %d and %d on query %d", source_node, msg_in.source_node, msg_in.query_id);
    add_joinpair(msg_in.query_id, msg_in.source_node, msg_in.path_nodecount, msg_in.path_nodes,
      source_node, path_nodecount, path_nodes, PAIROPT_INNET);  // allocate state to perform window join for this pair
#ifdef TOSSIM
    join_logpair(msg_in.query_id, msg_in.source_node, source_node);
#endif

    if (msg_in.source_node == TOS_NODE_ID || msg_in.path_nodecount > 0)
    {                                  // if target has decided to join at base, it will be notifying the source
      msg_out.cmd = CMDHI_CONFIRMSOURCE;
      msg_out.query_id = msg_in.query_id;
      msg_out.target_node = source_node; // let the source know about which target it is joining with
      msg_out.seq_number = msg_in.seq_number;
      msg_out.level = trees[0].hops;
      msg_out.sel_update = msg_in.sel_update;
      memcpy(msg_out.sel_est, msg_in.sel_est, sizeof(msg_in.sel_est));

      size = marshal_confirmsourcemsghi(&msg_out, buffer);
      init_forward(TOS_NODE_ID, TOS_NODE_ID, msg_in.source_node, msg_in.path_nodecount, msg_in.path_nodes,
        (TOS_NODE_ID == BASESTATION_ADDR) ? FORWARD_DESTRUCTIVE : FORWARD_NONDESTRUCTIVE, size, buffer, 0,
        NULL, ROUTE_SUCCESS);
    }
  }

  display_joinbuffer(); 
}

void resetjoin_base(ADDRESS join_node)   // resets join with join_node to Base or disables it if join_node is already Base
{
  int i;

DO_STACKTEST
  
  for (i = 0; i < QUERYSOURCEBUFFER_SIZE; i++)      // ... disable this join node and instead send to base
    if (querysource_buffer[i].used &&
      queryjointarget_buffer[querysource_buffer[i].jointarget_index].id == join_node)
    {  
      if (join_node == BASESTATION_ADDR)
      {                                             // if were already sending to base, then dont send anymore
        DBGOUT("Disabling join with %d because of failed base station!", querysource_buffer[i].id);
        querysource_buffer[i].jointarget_index = -1;
      } else {
        DBGOUT("To join with node %d will now send to the base station", querysource_buffer[i].id);
        querysource_buffer[i].jointarget_index = JOININDEX_BASE;
      }
    }  
}

void handlefail_confirmjoinmsghi(ADDRESS source_node, ADDRESS dest_node, uint8_t path_nodecount,
  ADDRESS *path_nodes, uint8_t data_len, uint8_t *data)
{                               // this handler is invoked at the target, if this msg fails to get to the join node
  DBGOUT("Join node %d has failed!", source_node);
  resetjoin_base(source_node); //shold try to get to source on some other path, not containing the failed node
}

void handle_sourcejoinupdate(uint8_t query_id, uint8_t seq_number, ADDRESS join_node,
  uint8_t join_level, uint8_t join_pathlen, ADDRESS *join_path, ADDRESS target_node,
  uint8_t target_pathlen, ADDRESS *target_path, uint8_t sel_update, uint8_t *sel_est)
{
  int querysource_index, queryjointarget_index;

DO_STACKTEST

  if (query_buffer[query_id].used == 0)
  {
    DBGERR("Query %d not used", query_id);
    return;
  }

  querysource_index = query_getsourcenode(query_id, target_node, REL_S, target_pathlen, target_path, NULL, 1);
  if (querysource_index < 0)
  {
    DBGERR("Cannot allocate entry in querysource_buffer");
    pause_sim();
    return;
  }

  queryjointarget_index = query_getjoinnode(join_node, join_pathlen, join_path, join_level);
  if (queryjointarget_index < 0)
  {
    DBGERR("Cannot allocate slot in queryjointarget_buffer");
    pause_sim();
    return;
  }

  if ((querysource_buffer[querysource_index].seq_number < seq_number &&
    seq_number < querysource_buffer[querysource_index].seq_number+JOINEST_SEQWINDOW) ||
    ((uint16_t) querysource_buffer[querysource_index].seq_number+JOINEST_SEQWINDOW >
    (uint16_t) seq_number+256))
  {                  // ensure in-order updates of join point
    DBGOUT("Will stream to join node %d to join with target %d on query %d", join_node,
      target_node, query_id);
#ifdef TOSSIM
    join_lognode(query_id, join_node, target_node);
#endif

    querysource_buffer[querysource_index].jointarget_index = queryjointarget_index;
    querysource_buffer[querysource_index].seq_number = seq_number;
    if (sel_update)
      update_pairselest(query_id, querysource_index, sel_est);
    group_opt(query_id, querysource_index);
    query_buffer[query_id].rel[querysource_buffer[querysource_index].rel_id].multicast.improved = 1;
  }
}

void handle_confirmsourcemsghi(ADDRESS source_node, ADDRESS dest_node, uint8_t path_nodecount,
  ADDRESS *path_nodes, uint8_t data_len, uint8_t *data)
{                                    // hanlder for msg received at the source node from the join node
  CONFIRMSOURCE_MSGHI msg_in;

DO_STACKTEST

  unmarshal_confirmsourcemsghi(&msg_in, data);
  handle_sourcejoinupdate(msg_in.query_id, msg_in.seq_number, source_node, msg_in.level,
    path_nodecount, path_nodes, msg_in.target_node, 0, NULL, msg_in.sel_update, msg_in.sel_est);
}

void handle_confirmtargetsourcemsghi(ADDRESS source_node, ADDRESS dest_node, uint8_t path_nodecount,
  ADDRESS *path_nodes, uint8_t data_len, uint8_t *data)
{
  CONFIRMTARGETSOURCE_MSGHI msg_in;

DO_STACKTEST

  unmarshal_confirmtargetsourcemsghi(&msg_in, data);
  handle_sourcejoinupdate(msg_in.query_id, msg_in.seq_number, BASESTATION_ADDR, 0, 0, NULL,
    source_node, path_nodecount, path_nodes, msg_in.sel_update, msg_in.sel_est);
}

void handlefail_confirmtargetsourcemsghi(ADDRESS source_node, ADDRESS dest_node, uint8_t path_nodecount,
  ADDRESS *path_nodes, uint8_t data_len, uint8_t *data)
{                               // this handler is invoked at the target, if this msg fails to get to the join node
  DBGOUT("Join node %d has failed!", source_node);
  resetjoin_base(source_node); //shold try to get to source on some other path, not containing the failed node
}

void handle_repairmsghi(ADDRESS source_node, ADDRESS dest_node, uint8_t path_nodecount,
  ADDRESS *path_nodes, uint8_t data_len, uint8_t *data) // handler invoked when RepairRoute succeeds
{
  REPAIR_MSGHI msg;
  uint8_t buffer[sizeof(msg)];
  int repair_pathindex, size;
  ADDRESS new_path[MAX_PATHLEN];

DO_STACKTEST

  unmarshal_repairmsghi(&msg, data);
  if (msg.path_found == 0)           // if we are a node on the portion of the path beyond the failure
  {                                  // we generate a message back to the node prior to the failure, on the repaired path
    msg.path_found = 1;

    DBGOUT("Notifying %d for found repair index %d, neighbor: %d, dest: %d", source_node,
      msg.path_index, msg.neighbor_node, msg.dest_node);
    size = marshal_repairmsghi(&msg, buffer);
    init_forward(TOS_NODE_ID, TOS_NODE_ID, source_node, path_nodecount, path_nodes,
      FORWARD_NONDESTRUCTIVE, size, buffer, 0, NULL, ROUTE_SUCCESS);
  } else {                           // if we are the node just prior to the failure, apply the repair
    DBGOUT("Repair path found from %d, index %d, neighbor: %d, dest: %d", source_node,
      msg.path_index, msg.neighbor_node, msg.dest_node);

    repair_pathindex = find_repairpathindex(msg.neighbor_node, msg.dest_node);
    if (repair_pathindex < 0)
    {
      DBGERR("Cannot find repair entry!");
      pause_sim();
      return;
    }

    if (repair_path[repair_pathindex].path_status != PATHSTATUS_REPAIRING)
    {
      DBGOUT("Path repair has already been performed (%d)",
        repair_path[repair_pathindex].path_status);
      return;
    }

    repair_path[repair_pathindex].path_status = PATHSTATUS_REPAIRED;

    memcpy(new_path, path_nodes, path_nodecount*sizeof(ADDRESS));
    memcpy(&new_path[path_nodecount], &repair_path[repair_pathindex].path[msg.path_index+1],
      (repair_path[repair_pathindex].path_len-msg.path_index-1)*sizeof(ADDRESS));
    repair_path[repair_pathindex].path_len = path_nodecount+
      repair_path[repair_pathindex].path_len-msg.path_index-1;

    mfree(repair_path[repair_pathindex].path);
    repair_path[repair_pathindex].path =
      (ADDRESS *) mcalloc(repair_path[repair_pathindex].path_len, sizeof(ADDRESS));
    memcpy(repair_path[repair_pathindex].path, new_path,
      repair_path[repair_pathindex].path_len*sizeof(ADDRESS));

#ifdef TOSSIM
    print_path(repair_path[repair_pathindex].path, repair_path[repair_pathindex].path_len,
      "Repair path");
#endif
                                         // resend the queued up messages (if any) on the repaired path
    handlerepair_forwardmsgs(ROUTE_SUCCESS, msg.neighbor_node, msg.dest_node);
  }
}

void stop_query(QUERY_ENTRY *p)
{
  int i;

DO_STACKTEST
  
  DBGOUT("Stopping query %d", p->query_id);
  set_globalstatus(GS_IDLE);

  for (i = 0; i < SRCREL_COUNT; i++)
  {
    mfree(p->rel[i].history_buffer);
    if (p->rel[i].multicast.buffer)
    {
      mfree(p->rel[i].multicast.buffer);
      p->rel[i].multicast.buffer = NULL;
    }
    if (p->rel[i].multicast.path)
    {
      mfree(p->rel[i].multicast.path);
      p->rel[i].multicast.path = NULL;
    }
    if (p->rel[i].multicast.prev_path)
    {
      mfree(p->rel[i].multicast.prev_path);
      p->rel[i].multicast.prev_path = NULL;
    }
  }

  for (i = 0; i < QUERYSOURCEBUFFER_SIZE; i++)
    if (querysource_buffer[i].used && querysource_buffer[i].query_id == p->query_id)
      querysource_buffer[i].used = 0;

  for (i = 0; i < QUERYJOINTARGETBUFFER_SIZE; i++)
    if (queryjointarget_buffer[i].used && jointarget_anymultiplicity(i) == 0)
      queryjointarget_buffer[i].used = 0;

  for (i = 0; i < MULTICASTCACHE_SIZE; i++)
    if (multicast_cache[i].used && multicast_cache[i].query_id == p->query_id)
      multicast_cache[i].used = 0;

  stopquery_windowjoin(p);
  p->used = 0;
}

void handle_querymsghi(ADDRESS source_node, ADDRESS dest_node, uint8_t path_nodecount,
  ADDRESS *path_nodes, uint8_t data_len, uint8_t *data)
{
  int i;
  QUERYSPECP_MSGHI msg_in;
  QUERY_ENTRY *p;
#if defined (TOSSIM) || defined (UNIT_TEST)
  int j, k;
  char s[256] = "";
#endif

DO_STACKTEST

  DBGOUT("Query message received from %d", source_node);
  msg_in.spec = NULL;        // do not unmarshal specification if any yet
  unmarshal_queryspecpmsghi(&msg_in, data);

  p = &query_buffer[msg_in.query_id];
  if (msg_in.running)
  {
    if (p->used)
    {
      DBGERR("Query %d already running", msg_in.query_id);
      pause_sim();
      return;
    }

    memset(p, 0, sizeof(QUERY_ENTRY));

    msg_in.spec = &p->spec;    // unmarshal specification directly in query buffer
    unmarshal_queryspecpmsghi(&msg_in, data);

    for (i = 0; i < p->spec.routingpredicate_count; i++)
      if (p->spec.routing_predicates[i].summary_index >= summarydesc_count)
      {
        DBGERR("Invalid summary index %d in routing predicate %d",
          p->spec.routing_predicates[i].summary_index, i);
        pause_sim();
        return;
      }

    p->used = 1;
    p->query_id = msg_in.query_id;
    p->options = config.defaultquery_options;
    p->initiated = 0;

    for (i = 0; i < REL_COUNT; i++)
    {
      p->tuple_len[i] = TUPLEID_LEN+p->spec.rel[REL_DYNAMIC][i].eval_count;
      p->aggrtuple_len[i] = p->tuple_len[i]+p->spec.rel[REL_STATIC][REL_J].eval_aggrcount;
      p->sel_est[i] = p->spec.sel_est[i];
    }

    if (p->spec.srcrel_count == 1)
    {
      p->tuple_len[REL_J] = 0;
      p->aggrtuple_len[REL_J] = 0;
    }

    for (i = 0; i < SRCREL_COUNT; i++)
    {
      p->rel[i].tuples_sent = 0;
      p->rel[i].nextsampling_cycle = p->spec.params[PARAM_BEGIN];
      p->rel[i].history_buffer =
        (ATTRIBUTE *) mmalloc(p->spec.params[PARAM_WINDOWSIZE]*p->tuple_len[i]*sizeof(ATTRIBUTE));
      memset(p->rel[i].history_buffer, 0,
        p->spec.params[PARAM_WINDOWSIZE]*p->tuple_len[i]*sizeof(ATTRIBUTE));
      p->rel[i].multicast.improved = 1;

      p->rel[i].est_tuplecount = 0;
      p->rel[i].est_starttime = p->spec.params[PARAM_BEGIN];  
    }

    p->groupopt.cost_updated = 0;
    p->groupopt.coordinator_updated = 0;
    p->groupopt.cost_received = 0;
    p->groupopt.localcoordinator_index = -1;   // no coordinator yet
    p->groupopt.globalcoordinator_node = 0xffff;
    p->groupopt.rcv_decisionseq = 0;
    p->groupopt.sent_decisionseq = 0;
    p->groupopt.decision = DECISION_INNET;   // by default, do INNET join

#if defined (TOSSIM) || defined (UNIT_TEST)
    if (TOS_NODE_ID == BASESTATION_ADDR)
    {
      DBGOUT("Parsed query %d:", msg_in.query_id);
      DBGOUT("Srcsel count: %d", p->spec.srcrel_count);      
      for (j = 0; j < REL_COUNT; j++)                  // display results
      {
        DBGOUT("Relation %s", rel_name[j]);
        for (k = REL_STATIC; k <= REL_DYNAMIC; k++)
        {
          DBGOUT("%s predicate", (k == REL_STATIC) ? "Static" : "Dynamic");
          print_stack(&p->spec.rel[k][j].pred);
          DBGOUT("Id index: %d", p->spec.rel[k][j].id_index);
          for (i = 0; i < p->spec.rel[k][j].eval_count; i++)
          {
            DBGOUT("%s evaluation %s.temp%d %s", (k == REL_STATIC) ? "Static" : "Dynamic",
              rel_name[j], i, p->spec.rel[k][j].eval[i].is_aggr ? "(aggr)" : "");
            print_stack(&p->spec.rel[k][j].eval[i].eval);
          }
        }
      }

      DBGOUT("Routing:");
      for (i = 0; i < p->spec.routingpredicate_count; i++)
      {
        DBGOUT("Predicate %ld on %s(%d), offset: %d", i,
          attr_name[summary_desc[p->spec.routing_predicates[i].summary_index].attrdesc_index],
          p->spec.routing_predicates[i].summary_index,
          p->spec.routing_predicates[i].lastchild_ofs);
        print_stack(&p->spec.routing_predicates[i].pred);
      }

      for (i = 0; i < PARAM_COUNT; i++)
      {
        j = strlen(s);
        sprintf(&s[j], "%ld ", p->spec.params[i]);
      }
      DBGOUT("Parameters: %s", s);
      DBGOUT("Sel est: (%d, %d, %d)", p->spec.sel_est[REL_S], p->spec.sel_est[REL_T],
        p->spec.sel_est[REL_J]);
      DBGOUT("Tuple len: (%d, %d, %d), aggr: (%d, %d, %d)", p->tuple_len[REL_S], p->tuple_len[REL_T],
        p->tuple_len[REL_J], p->aggrtuple_len[REL_S], p->aggrtuple_len[REL_T], p->aggrtuple_len[REL_J]);
    }
#endif
  } else if (p->used)
    stop_query(p);
  else DBGERR("Query %d not used", msg_in.query_id);  
}

void handle_treesdonemsghi(ADDRESS source_node, ADDRESS dest_node, uint8_t path_nodecount,
  ADDRESS *path_nodes, uint8_t data_len, uint8_t *data)
{
DO_STACKTEST

  if (dest_node == BASESTATION_ADDR)
  {
    DBGOUT("Queueing trees done message");
    trees_built = 1;
    enqueue_uartmsg(1, data_len, data, source_node);
  } else {
    DBGERR("Invalid destination of trees done message");
    pause_sim();
    return;
  }
}

void handlerouteinter_init(ROUTE_MSG *msg)
{                                        // perform intermediate operation during routing when we carry a establish_long payload
  INIT_MSGHI msghi;             // we maintain path levels, taking into account shortcuts on the routing layer

DO_STACKTEST

  unmarshal_initmsghi(&msghi, msg->data);

  msghi.path_len = msg->path_len;
  msghi.path_levels[msghi.path_len-1] = trees[0].hops;
  if (msg->shortcut_index >= 0 && msghi.path_len >= 2) // we shouldnt have a shortcut if we have 2 hops or less, but just in case
  {
    msghi.path_levels[msghi.path_len-2] = peer_link[msg->shortcut_index].tree.trees[0].hops;
    msg->shortcut_index = -1;
  }

  msg->data_len = marshal_initmsghi(&msghi, msg->data);
#ifdef TOSSIM
  print_list(msg->data, msg->data_len, "Updated data");
#endif
}

void handlerouteinter_repair(ROUTE_MSG *msg)
{                                  // perform intermediate operation during routing when we carry a repair payload
  REPAIR_MSGHI msghi;              // we shouldnt be performing shortcuts when on repair, but just in case...

DO_STACKTEST

  unmarshal_repairmsghi(&msghi, msg->data);
  if (msg->shortcut_index >= 0)
    msghi.path_index = msg->shortcut_index;

  msg->data_len = marshal_repairmsghi(&msghi, msg->data);
#ifdef TOSSIM
  print_list(msg->data, msg->data_len, "Updated data");
#endif
}

uint32_t evalpath_init(ROUTE_MSG *msg)
{
  int i, min = 0;
  INIT_MSGHI msghi;

DO_STACKTEST

  unmarshal_initmsghi(&msghi, msg->data);
  for (i = 1; i < msghi.path_len; i++)
    if (msghi.path_levels[i] < msghi.path_levels[min])
      min = i;

  return (10*msg->path_len+msghi.path_levels[min]);
}

uint8_t queryplan_resolvepath(ADDRESS dest_node, ADDRESS *next_hop, ADDRESS *path, uint8_t *path_len)
{
  int i;

DO_STACKTEST
  
  for (i = 0; i < QUERYJOINTARGETBUFFER_SIZE; i++)
    if (queryjointarget_buffer[i].used && queryjointarget_buffer[i].id == dest_node &&
      queryjointarget_buffer[i].path_nodecount > 0)
    {
      *path_len = queryjointarget_buffer[i].path_nodecount;
      memcpy(path, queryjointarget_buffer[i].path_nodes, *path_len*sizeof(ADDRESS));
      *next_hop = path[0];
      DBGOUT("Next hop retrieved from join target buffer: %d", *next_hop);
      
      return 1;
    }

  return windowjoin_resolvepath(dest_node, next_hop, path, path_len);
}

uint8_t jointarget_multiplicity(QUERY_ENTRY *q, uint8_t rel_id, int jointarget_index)
{
  int i;
  uint8_t sum = 0;

  for (i = 0; i < QUERYSOURCEBUFFER_SIZE; i++)
    if (querysource_buffer[i].used && querysource_buffer[i].query_id == q->query_id &&
      querysource_buffer[i].rel_id == rel_id && querysource_buffer[i].jointarget_index == jointarget_index)
      sum++;

  return sum;
}

uint8_t jointarget_anymultiplicity(int jointarget_index)
{
  int i, k;

  for (k = 0; k < QUERYBUFFER_SIZE; k++)
    if (query_buffer[k].used)
      for (i = 0; i < SRCREL_COUNT; i++)
        if (jointarget_multiplicity(&query_buffer[k], i, jointarget_index) > 0)
          return 1;

  return 0;
}

void check_joinpathprefix(QUERY_ENTRY *q, uint8_t rel_id, int path_index)
{              // for a given path to be inserted in the multiast tree
  int i;       // enforce sharing of prefixes with paths already in the multicast tree

  for (i = 0; i < path_index; i++)
    if (queryjointarget_buffer[i].used && jointarget_multiplicity(q, rel_id, i) > 0)
      share_pathprefix(queryjointarget_buffer[i].path_nodecount, queryjointarget_buffer[i].path_nodes,
        &queryjointarget_buffer[path_index].path_nodecount,
        &queryjointarget_buffer[path_index].path_nodes);
}

void build_multicasttree(QUERY_ENTRY *q, uint8_t rel_id)   // build a multicast tree of paths to join nodes
{                            // exclude certain join nodes such as Base, join at self, and short paths
  int i, path_count = 0;
  MULTICAST_NODE *root = NULL;
  uint8_t size, *p, *p1;
  ADDRESS path[MAX_PATHLEN];
  uint8_t path_len;
  MULTICAST_ENTRY *pm;

DO_STACKTEST

  pm = &q->rel[rel_id].multicast;
  pm->buffer_len = 0;
  if (pm->buffer)
    mfree(pm->buffer);
  pm->buffer = NULL;
  pm->path_len = 0;
  if (pm->path)
    mfree(pm->path);
  pm->path = NULL;

  for (i = 0; i < QUERYJOINTARGETBUFFER_SIZE; i++)   // find and insert suitable paths into the multicast tree
    if (queryjointarget_buffer[i].used && jointarget_multiplicity(q, rel_id, i) > 0)
    {
      path[0] = TOS_NODE_ID;
      if (queryjointarget_buffer[i].id == BASESTATION_ADDR)
      {
        path_len = 2;
        path[1] = BASESTATION_ADDR;
      } else {
        check_joinpathprefix(q, rel_id, i);
        path_len = queryjointarget_buffer[i].path_nodecount+1;
        memcpy(&path[1], queryjointarget_buffer[i].path_nodes, (path_len-1)*sizeof(ADDRESS));
      }

      multicast_insertpath(&root, path, path_len, queryjointarget_buffer[i].level,
        jointarget_multiplicity(q, rel_id, i));
      path_count++;

#ifdef TOSSIM
      print_path(&path[1], path_len-1, "Added path");
#endif
    }

  if (path_count > 1)       // if we have more than one path in the tree, marshal it into a multicast buffer
  {
    p = NULL; p1 = NULL;
    marshal_checksize = 1;
    marshal_multicast(&p, root, 0);
    marshal_checksize = 0;
    size = p-p1;
    DBGOUT("Estimated multicast size: %d", size);

    if (size <= MULTICASTBUFFER_SIZE)
    {
      pm->buffer_len = size;
      pm->buffer = (uint8_t *) mmalloc(size);
      p = pm->buffer;
      marshal_multicast(&p, root, 0);

      pm->path_len = root->path_len-1;
      if (pm->path_len > 0)
      {
        pm->path = (ADDRESS *) mcalloc(pm->path_len, sizeof(ADDRESS));
        memcpy(pm->path, &root->path[1], pm->path_len*sizeof(ADDRESS));
      } else pm->path = NULL;

      pm->node = (pm->path_len > 0) ? pm->path[pm->path_len-1] : TOS_NODE_ID;
      pm->cost = estimate_multicastcost(root, (q->options & QUERYOPT_CACHEMULTICAST) != 0, rel_id,
        q->sel_est, q->tuple_len, q->spec.params[PARAM_WINDOWSIZE], 0);

#ifdef TOSSIM
      multicast_print(root, 0);
      DBGOUT("Multicast node: %d", pm->node);
      print_path(pm->path, pm->path_len, "Multicast path");
      print_list(pm->buffer, pm->buffer_len, "Multicast buffer");
#endif
    } else {
      DBGERR("Multicast size too large");
      pm->cost = 0;
    }
  } else
    DBGOUT("Less than 2 paths in multicast");

  DBGOUT("Multicast cost: %d", pm->cost);
  print_sel(q->sel_est);
  
  if (root)
    multicast_free(root);
}

void optimize_plan()  // evaluate node-centric plan
{
  int i, j, k;
  uint8_t jp_exists;
  QUERY_ENTRY *p;

DO_STACKTEST

  for (k = 0; k < QUERYBUFFER_SIZE; k++)
    if (query_buffer[k].used)
    {
      p = &query_buffer[k];
      DBGOUT("Query %d", p->query_id);
      for (j = 0; j < SRCREL_COUNT; j++)
      {
#ifdef TOSSIM
        DBGOUT("Relation %s", rel_name[j]);
#endif
        
        p->rel[j].multicast.cost = 0;
        if (p->options & QUERYOPT_MULTICAST)
          build_multicasttree(p, j);
        p->rel[j].multicast.innet_cost = p->rel[j].multicast.cost;

        jp_exists = 0;
        for (i = 0; i < QUERYJOINTARGETBUFFER_SIZE; i++)
          if (queryjointarget_buffer[i].used && jointarget_multiplicity(p, j, i) > 0)
          {
            jp_exists = 1;
            if (p->rel[j].multicast.buffer_len == 0)  // no multicast buffer created, score join nodes separately
            {
#ifdef TOSSIM
              DBGOUT("Evaluating join node %d", queryjointarget_buffer[i].id);
              print_path(queryjointarget_buffer[i].path_nodes,
                queryjointarget_buffer[i].path_nodecount, "  Path");
#endif
              p->rel[j].multicast.innet_cost += estimate_singlejoincost(
                (queryjointarget_buffer[i].id == BASESTATION_ADDR),
                queryjointarget_buffer[i].path_nodecount, NULL, 0, trees[0].hops,
                queryjointarget_buffer[i].level, jointarget_multiplicity(p, j, i), j,
                p->sel_est, p->tuple_len, p->spec.params[PARAM_WINDOWSIZE], 0);
            }
          }

        p->rel[j].multicast.innet_seq++;  // update per-relation
        p->innet_seq++;                   // and global sequence

        DBGOUT("In-net cost: %d", p->rel[j].multicast.innet_cost);
        print_sel(p->sel_est);

        if (jp_exists)  // there exists at least one join point
        {
          p->rel[j].multicast.base_cost = estimate_singlejoincost(1, 0, NULL, 0, trees[0].hops,
            0, 1, j, p->sel_est, p->tuple_len, p->spec.params[PARAM_WINDOWSIZE], 0);

          DBGOUT("Base cost: %d", p->rel[j].multicast.base_cost);
          print_sel(p->sel_est);
        }
      }
    }
}



